import sys
import time

import ecal.core.core as ecal_core
from ecal.core.publisher import ProtoPublisher

# Import the "hello_world_pb2.py" file that we have just generated from the
# proto_messages directory 

sys.path.append('/media/guo/3AFE19F8FE19ACDF/SMILES/mower_mpc/proto/protobuf')

import localization_pb2 as lat_longitude_data_pb2
import serial
import re

utctime = ''
lat = ''
ulat = ''
lon = ''
ulon = ''
numSv = ''
msl = ''
cogt = ''
cogm = ''
sog = ''
kph = ''
gps_t = 0

ser = serial.Serial("/dev/ttyACM1", 115200)

if ser.isOpen():
    print("GPS Serial Opened! Baudrate=115200")
else:
    print("GPS Serial Open Failed!")


def Convert_to_degrees(in_data1, in_data2):
    len_data1 = len(in_data1)
    str_data2 = "%05d" % int(in_data2)
    temp_data = int(in_data1)
    symbol = 1
    if temp_data < 0:
        symbol = -1
    degree = int(temp_data / 100.0)
    str_decimal = str(in_data1[len_data1-2]) + str(in_data1[len_data1-1]) + str(str_data2)
    f_degree = int(str_decimal)/60.0/100000.0
    # print("f_degree:", f_degree)
    if symbol > 0:
        result = degree + f_degree
    else:
        result = degree - f_degree

    return result

def GPS_read(utctime,lat,ulat,lon,ulon,numSv,msl,cogt,cogm,sog,kph,gps_t):

    #GGA = ser.read(700)#GNGGA,064839.00,3111.27952,N,12034.98476
    #print('GGA',GGA)
    if ser.inWaiting():
        #print('ser.inWaiting()')
        GGA = ser.read(ser.inWaiting())#GNGGA,064839.00,3111.27952,N,12034.98476
        print('GGA',GGA)
        with open('RTK_original_data.txt', 'a') as f:

            f.write(str(GGA)+'\n')
        GGA_g = re.findall(r"\w+(?=,)|(?<=,)\w+", str(GGA))
        #print('GGA_gd',GGA_g)
        if GGA_g[0]=='GNRMC':
            utctime = GGA_g[1]
            # lat = GGA_g[2][0]+GGA_g[2][1]+'°'+GGA_g[2][2]+GGA_g[2][3]+'.'+GGA_g[3]+'\''
            #lat = "%.8f" % Convert_to_degrees(str(GGA_g[4]), str(GGA_g[5]))
            try:
                lat= Convert_to_degrees(str(GGA_g[4]), str(GGA_g[5]))

                ulat = GGA_g[6]
                # lon = GGA_g[5][0]+GGA_g[5][1]+GGA_g[5][2]+'°'+GGA_g[5][3]+GGA_g[5][4]+'.'+GGA_g[6]+'\''
                lon = Convert_to_degrees(str(GGA_g[7]), str(GGA_g[8]))
                ulon = GGA_g[9]
                numSv = ' '
                msl = GGA_g[12]+'.'+GGA_g[13]+GGA_g[14]
                #print(GGA_g)
                gps_t = 1
                return utctime, lat, ulat, lon, ulon, numSv, msl, cogt, cogm, sog, kph, gps_t
            except:
                print('bad signal!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')
                return utctime, lat, ulat, lon, ulon, numSv, msl, cogt, cogm, sog, kph, gps_t
    return utctime, lat, ulat, lon, ulon, numSv, msl, cogt, cogm, sog, kph, gps_t
if __name__ == "__main__":
    # initialize eCAL API. The name of our Process will be
    # "Python Protobuf Publisher"
    ecal_core.initialize(sys.argv, "Python Protobuf Publisher")

    # Create a Protobuf Publisher that publishes on the topic
    # "hello_world_python_protobuf_topic". We also pass it the datatype we are
    # going to send later. By doing this we enable the eCAL Monitor to dynamically
    # show the content of the messages.
    pub = ProtoPublisher("hello_world_python_protobuf_topic"
                        , lat_longitude_data_pb2.localizationGNGGA)

    # Create a counter and some messages, so we see something changing in the
    # message, later
    counter = 0
    dummy_messages = ["The story so far:"
                    , "In the beginning the Universe was created."
                    , "This has made a lot of people very angry"
                    , "and been widely regarded as a bad move."]

    # Infinite loop (using ecal_core.ok() will enable us to gracefully shutdown
    # the process from another application)
    while ecal_core.ok():
        utctime,lat,ulat,lon,ulon,numSv,msl,cogt,cogm,sog,kph,gps_t=GPS_read(utctime,lat,ulat,lon,ulon,numSv,msl,cogt,cogm,sog,kph,gps_t)

        # Create a message and fill it with some data
        protobuf_message = lat_longitude_data_pb2.localizationGNGGA()
        #protobuf_message.name = "latitude longitude"
        if lat!='':
            protobuf_message.latitude   =lat
            protobuf_message.longitude  = lon

        print("protobuf_message.latitude",lat)
        print("protobuf_message.longitude",lon)
        # actually send the message to the topic this publisher was created for
        pub.send(protobuf_message)

        # Sleep 1s
        time.sleep(1)

        counter = counter + 1

        # finalize eCAL API
    ecal_core.finalize()
